/*
 * @Author: laladuduqq
 * @Date: 2024-12-22 17:10:55
 * @LastEditors: laladuduqq
 * @LastEditTime: 2025-01-13 14:49:43
 */
#include "signal_create.h"
#include "arm_math.h"
#include "rtthread.h"
#include <math.h>

// 计算正弦运动角度
float calculate_sinusoidal_motion(SinusoidalMotionParams params, rt_tick_t current_tick)
{
    float time_in_seconds = (float)current_tick / RT_TICK_PER_SECOND;
    float normalized_time = fmod(time_in_seconds, params.period / 1000.0);  // 归一化时间
    float angle_range = params.end_angle - params.start_angle;
    float angle = params.start_angle + angle_range * 0.5 * (1.0 + sin(2.0 * PI * normalized_time / (params.period / 1000.0)));
    return angle;
}


// 定义正弦波参数
#define AMPLITUDE 50.0f    // 正弦波的幅度
#define FREQUENCY 1.0f     // 正弦波的频率 (rad/s)
#define OFFSET 0.0f        // 正弦波的偏移量

// 生成正弦波的目标角度
float generate_sine_wave(float time) {
    return OFFSET + AMPLITUDE * sin(FREQUENCY * time);
}

/**
 * @brief          正弦信号发生器
 * @param[in]      amplitude 振幅
 * @param[in]      offset 输出偏移量
 * @param[in]      period (s)周期
 * @return         SinWave
 */
float GenerateSinWave(float amplitude, float offset, float period)
{
    float t = rt_tick_get() / 1000.0f;
    return amplitude * sin((2 * PI / period) * t) + offset;
}

/**
 * @brief          阶跃信号发生器
 * @param[in]      a0 初始值
 * @param[in]      a1 最终值
 * @param[in]      t0 (s)起始时间
 * @return         StepWave
 */
float GenerateStepWave(float a0, float a1, float t0)
{
    float t = rt_tick_get()  / 1000.0f;
    return (t >= t0) ? a1 : a0;
}

/**
 * @brief          生成斜坡波信号
 * @param[in]      a0 信号的初始幅度
 * @param[in]      a1 信号的最终幅度
 * @param[in]      t0 (s)信号的起始时间
 * @param[in]      t1 (s)信号的结束时间
 * @return         当前时间下的信号幅度
 */
float GenerateRampWave(float a0, float a1, float t0, float t1)
{
    float t = rt_tick_get() / 1000.0f;
    return (t >= t0 && t <= t1) ? a0 + (a1 - a0) * (t - t0) / (t1 - t0) : a0;
}

/**
 * @brief          生成脉冲波形信号
 * @param[in]      a1 脉冲波形的幅值1
 * @param[in]      a2 脉冲波形的幅值2
 * @param[in]      t1 (s)脉冲幅值1的持续时间
 * @param[in]      t2 (s)脉冲幅值2的持续时间
 * @return         当前时间在脉冲波形时间范围内时返回幅值1，否则返回幅值0
 */
float GeneratePulseWave(float a1, float a2, float t1, float t2)
{
    uint32_t t = rt_tick_get() ;
    uint32_t t1_ = t1 * 1000;
    uint32_t t2_ = t2 * 1000;
    t = t % (t1_ + t2_);
    return (t < t1_) ? a1 : a2;
}

/**
 * @brief          生成锯齿波形
 * @param[in]      a0 波形起始值
 * @param[in]      a1 波形结束值
 * @param[in]      T (s)波形周期
 * @return         生成的锯齿波形值
 */
float GenerateSawtoothWave(float a0, float a1, float T)
{
    float t = rt_tick_get()  / 1000.0f;
    return a0 + (a1 - a0) * (t / T - floor(t / T));
}

void LinearMotionInit(LinearMotionParams_t *params, float min_pos, float max_pos)
{
    params->min_position = min_pos;
    params->max_position = max_pos;
    params->frequency = 0.1f;
    params->direction = 1.0f;
    params->start_time = 0.0f;
    params->is_moving = RT_TRUE;
    params->current_position = 0.0f;
}

float LinearMotionUpdate(LinearMotionParams_t *params)
{
    float ref = params->current_position;
    if (params->is_moving)
    {
        float current_time = rt_tick_get();
        float elapsed_time = (current_time - params->start_time) / RT_TICK_PER_SECOND;
        ref = params->current_position + params->direction * (params->max_position - params->min_position) * (elapsed_time * params->frequency);
        if (ref > params->max_position)
        {
            ref = params->max_position;
            params->direction = -1.0f;
            params->start_time = current_time;
        }
        else if (ref < params->min_position)
        {
            ref = params->min_position;
            params->direction = 1.0f;
            params->start_time = current_time;
        }
    }
    return ref;
}

void LinearMotionStart(LinearMotionParams_t *params, float current_position)
{
    params->start_time = rt_tick_get();
    params->current_position = current_position;
    params->is_moving = RT_TRUE;
}

void LinearMotionStop(LinearMotionParams_t *params)
{
    params->is_moving = RT_FALSE;
}


void SinusoidalMotionInit(SinusoidalMotionParams_t *params, float min_pos, float max_pos, float frequency)
{
    params->min_position = min_pos;
    params->max_position = max_pos;
    params->frequency = frequency; // 正弦波的频率
    params->start_time = 0.0f;
    params->is_moving = RT_TRUE;
    params->current_position = 0.0f;
}

float SinusoidalMotionUpdate(SinusoidalMotionParams_t *params)
{
    float ref = params->current_position;
    if (params->is_moving)
    {
        float current_time = rt_tick_get();
        float elapsed_time = (current_time - params->start_time) / RT_TICK_PER_SECOND;
        float mid_position = (params->max_position + params->min_position) / 2.0f;
        float range = (params->max_position - params->min_position) / 2.0f;
        ref = mid_position + range * sin(2 * PI * params->frequency * elapsed_time);
    }
    return ref;
}

void SinusoidalMotionStart(SinusoidalMotionParams_t *params, float current_position)
{
    params->start_time = rt_tick_get();
    params->current_position = current_position;
    params->is_moving = RT_TRUE;
}

void SinusoidalMotionStop(SinusoidalMotionParams_t *params)
{
    params->is_moving = RT_FALSE;
}
